#include "rclcpp/rclcpp.hpp"
#include "Serial32.h"
#include <chrono>
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/range.hpp"

using namespace std::chrono_literals;


/**
 * 定时器回调函数
 */
void Serial32::timer_callback()
{
    // 读取串口数据
    if (Read_SerialData()) {
        pub_alt();
        RCLCPP_INFO(this->get_logger(), "%f", Alt);
    }

    SendCheck();  // 检查发送
}


/**
 * cmd_vel回调函数
 */

void Serial32::cmd_vel_callback(geometry_msgs::msg::Twist::SharedPtr msg)
{
    RCLCPP_INFO(this->get_logger(), "Received cmd_vel: Linear x: '%f', Linear y: '%f', Linear z: '%f',Angular z: '%f'", 
    msg->linear.x, msg->linear.y, msg->linear.z,  msg->angular.z);

    uint8_t _buf[10];
    uint8_t _cnt = 0;
    int16_t _val = 0;
    
    _val = (msg->linear.x * 100);
    memcpy(_buf + _cnt, (uint8_t*)(&_val), 2);
    _cnt += 2;
    
    _val = (msg->linear.y * 100);
    memcpy(_buf + _cnt, (uint8_t*)(&_val), 2);
    _cnt += 2;
    
    _val = (msg->linear.z * 100);
    memcpy(_buf + _cnt, (uint8_t*)(&_val), 2);
    _cnt += 2;
    
    _val = (uint8_t)(msg->angular.z * 180 / 3.1415926);
    memcpy(_buf + _cnt, (uint8_t*)(&_val), 2);
    _cnt += 2;
    
    SendBuf(0x31,_buf,sizeof(_buf));
}


int main(int argc, char **argv)
{
    setlocale(LC_ALL,"");
    rclcpp::init(argc, argv);
    auto serial_node = std::make_shared<Serial32>("/dev/ttyS9", 921600, 1000, "Serial32");

    rclcpp::spin(serial_node);

    rclcpp::shutdown();
    return 0;
}


